//
// Created by Zemon on 2024/10/31.
//

#include "chassis_task.h"
#include "CAN_receive.h"
#include "bsp_can.h"
#include "motor_pid.h"
#include "remote_control.h"
#include "main.h"

pid_t pid_speed;
static float kp = 5.0f, ki = 0.0f, kd = 0.0f;
extern motor_measure_t motor_chassis[5];
int16_t chassis_speed[2];
int16_t pid_v[2];
extern RC_ctrl_t rc_ctrl;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim8;

float set_speed_temp;
int16_t delta;
int16_t max_speed_change = 1000;

void chassis_control(void)
{
    chassis_speed[0] = rc_ctrl.rc.ch[3]*5+rc_ctrl.rc.ch[2]*5;//左轮
    chassis_speed[1] = -rc_ctrl.rc.ch[3]*5+rc_ctrl.rc.ch[2]*5;//右轮
}

void chassis_move_back(void)
{
    PID_struct_init(&pid_speed,POSITION_PID,16384,16384,kp,ki,kd);

   for(int i=0;i<2;i++) {
          delta = chassis_speed[i] - motor_chassis[i].speed_rpm;
          //delta = (int16_t)chassis_speed[i] - motor_chassis[i].speed_rpm;
          if(delta>max_speed_change)
              set_speed_temp = (float)(motor_chassis[i].speed_rpm + max_speed_change);
          else if(delta<-max_speed_change)
              set_speed_temp = (float)(motor_chassis[i].speed_rpm - max_speed_change);
          else
              set_speed_temp = chassis_speed[i];
          pid_calc(&pid_speed,(float)motor_chassis[i].speed_rpm,set_speed_temp);
          pid_v[i] = (int16_t)(pid_speed.pos_out);
      }
      CAN_cmd_chassis(pid_v[0],pid_v[1]);


}

void chassis_move_front(void){
    if(chassis_speed[0]>=0){
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,0);
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,chassis_speed[0]*20);
    }
    else if(chassis_speed[0]<0){
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,-chassis_speed[0]*20);
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,0);
    }

    if(-chassis_speed[1]>=0){
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,-chassis_speed[1]*20);
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,0);
    }
    else if(-chassis_speed[1]<0){
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,0);
        __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,chassis_speed[1]*20);
    }

}

void chassis_move(void){
    chassis_control();
    chassis_move_front();
    chassis_move_back();
}